
#ifndef CORRESPONDENCE_TRANSFORM_H_
#define CORRESPONDENCE_TRANSFORM_H_
#include <Eigen/Core>

/**
 * 将世界坐标系中的点转换为占用栅格坐标系中的单元格索引。
 *
 * @param point 世界坐标系中的点，使用Eigen库的二维向量表示。
 * @param origin 占用栅格地图的原点在世界坐标系中的位置，同样使用Eigen库的二维向量表示。
 * @param resolution 占用栅格地图的分辨率，即每个栅格单元对应的现实世界长度。
 * @return 返回占用栅格坐标系中的单元格索引，使用Eigen库的二维数组表示。
 *
 * 此函数的主要作用是将一个在世界坐标系下的点转换到占用栅格坐标系下对应的单元格索引。
 * 通过减去原点坐标并除以分辨率，可以得到相对于栅格地图原点的单元格位置。
 * 由于栅格索引是整数，因此需要使用std::lround函数对浮点数结果进行四舍五入。
 */
Eigen::Array2i World2OccpancyCell(const Eigen::Vector2f& point, const Eigen::Vector2f& origin, float resolution) {
    return Eigen::Array2i{std::lround((point.x() - origin.x()) / resolution - 0.5),
                          std::lround((point.y() - origin.y()) / resolution - 0.5)};
}

#endif  // CORRESPONDENCE_TRANSFORM_H_